//========================================================================
//  This software is free: you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License Version 3,
//  as published by the Free Software Foundation.
//
//  This software is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public License
//  Version 3 in the file COPYING that came with this distribution.
//  If not, see <http://www.gnu.org/licenses/>.
//========================================================================
/*!
\file    pointing_engine.cpp
\brief   C++ Implementation: PointingEngine
\author  Joydeep Biswas, (C) 2011
*/
//========================================================================

#include "pointing_engine.h"

const double PointingEngine::MaxElbowMovement = 0.1;
const double PointingEngine::MinCosineMove = cos(RAD(10.0));
const double PointingEngine::MinCalibrationHover = 1.0;

PointingEngine::PointingEngine()
{
  conditionNumbers[0] = conditionNumbers[1] = conditionNumbers[2] = 0.0;
  calibrationState = -1;
  lastUserNum = -1;
  displayMode = 1;
  I = Matrix3d::Identity();
  timeoutStart = 0;
  for(int i=0; i<4; i++)
    calibrationPoints[i].zero();
  //Try to load last calibrated points
  loadCalibration();
}

PointingEngine::~PointingEngine()
{

}

void PointingEngine::getCalibration(vector3d _calibrationPoints[4], double _conditionNumbers[3], vector< vector3d >& _dataPoints, vector< vector3d >& _dataVectors)
{
  for(int i=0; i<3; i++){
    _calibrationPoints[i] = calibrationPoints[i];
    _conditionNumbers[i] = conditionNumbers[i];
  }
  _calibrationPoints[3] = calibrationPoints[3];
  
  _dataPoints = dataPoints;
  _dataVectors = dataVectors;
}

void PointingEngine::startCalibration(int pointNum)
{
  if(pointNum<0 || pointNum>2)
    calibrationState = -1;
  else{
    calibrationState = pointNum;
    dataPoints.clear();
    dataVectors.clear();
  }
}

void PointingEngine::stopCalibration()
{
  calibrationState = -1;
}

bool PointingEngine::getPointingTarget(vector2d& _target, vector3d &_target3d, double hoverDuration)
{
  _target = target;
  _target3d = target3d;
  hoverDuration = this->hoverDuration;
  return true;
}

void PointingEngine::observe(NUI_SKELETON_FRAME* skeletons)
{
  static const bool debug = true;

  userFound = false;
  userTracked = false;
  int userNum = lastUserNum;
  if(lastUserNum>=0){
    // See if the user still exists
    if(skeletons->SkeletonData[lastUserNum].eTrackingState == NUI_SKELETON_TRACKED && skeletons->SkeletonData[lastUserNum].dwQualityFlags==0){
      userTracked = true;
      userFound = true;
      qualityFlags = 0;
    }else{
      lastUserNum = -1; 
    }
  }
  for(int i=0; i<NUI_SKELETON_COUNT && !userTracked; i++){
    const NUI_SKELETON_DATA &skeleton = skeletons->SkeletonData[i];
    if(skeleton.eTrackingState == NUI_SKELETON_TRACKED && skeleton.dwQualityFlags==0){
      // Skeleton is being tracked, and is completely visible
      userFound = true;
      userTracked = true;
      qualityFlags = 0;
      userNum = i;
    }
    if(skeleton.eTrackingState == NUI_SKELETON_TRACKED){
      qualityFlags = skeleton.dwQualityFlags;
      userFound = true;
    }
  }
  
  // Can't do anything if the user's not obseravble now, can we?
  if(!userTracked)
    return;
  
  const NUI_SKELETON_DATA &skeleton = skeletons->SkeletonData[userNum];
  elbowLoc.set(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_ELBOW_RIGHT]));
  shoulderLoc.set(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT]));
  handLoc.set(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT]));
  pointingVector = (handLoc-shoulderLoc).norm();
  
  userSwitched = lastUserNum!=userNum;
  
  if(userSwitched){
    // Reset hover duration and history of states
    firstElbowLoc = elbowLoc;
    firstPointingVector = pointingVector;
    hoverStart = GetTimeSec();
  }else{
    // check if the user is "hovering" (roughly standing still)
    if( (elbowLoc-firstElbowLoc).sqlength()<sq(MaxElbowMovement) && firstPointingVector.dot(pointingVector)>MinCosineMove ){
      //Hovering
      hoverDuration = GetTimeSec()-hoverStart;
    }else{
      firstElbowLoc = elbowLoc;
      firstPointingVector = pointingVector;
      hoverStart = GetTimeSec();
    }
  }
  lastUserNum = userNum;
  
  if(calibrationState<0){
    // Normal operation: Compute point being pointed at 
    updatePointingTarget();
    return;
  }
  if(debug) printf("ElbowMoved:%d AngleChanging:%d\n",
    (elbowLoc-firstElbowLoc).sqlength()<sq(MaxElbowMovement)?0:1,
    firstPointingVector.dot(pointingVector)>MinCosineMove?0:1);
  recomputeCalibration();
}

void PointingEngine::recomputeCalibration()
{
  static const bool debug = true;
  // Check if the user is "pointing"
  bool pointing = (handLoc-elbowLoc).norm().dot((elbowLoc-shoulderLoc).norm()) > cos(RAD(5.0));
  if(debug) printf("pointing:%d\n",pointing?1:0);
  if(!pointing)
    return;

  // If hovering, add to dataPoints and dataVectors
  if(hoverDuration>MinCalibrationHover){ //&& (GetTimeSec()-timeoutStart)>2.0){
    dataPoints.push_back(shoulderLoc);
    dataVectors.push_back(pointingVector);
    //Beep(500, 500);
    #ifdef _WIN32
    MessageBeep(MB_OK);
    #endif
    // Reset hover duration and history of states
    firstElbowLoc = elbowLoc;
    firstPointingVector = pointingVector;
    hoverStart = GetTimeSec();
    timeoutStart = GetTimeSec();
  }


  // Compute matrices and pseudo-inverse
  A.setZero();
  B.setZero();
  for(unsigned int i=0; i<dataVectors.size(); i++){
    Vector3d v(V3COMP(dataVectors[i]));
    Matrix3d m = I-v*Transpose<Vector3d>(v);
    A += m;
    B += m*Vector3d(V3COMP(dataPoints[i]));
  }
  
  if(debug) printf("Calibrating using %d points\n", int(dataPoints.size()));
  // Recompute calibration point, condition number
  JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
  p = svd.solve(B);
  Vector3d s = svd.singularValues();
  conditionNumbers[calibrationState] = fabs(s.minCoeff()/s.maxCoeff());
  calibrationPoints[calibrationState] = vector3d(p.x(), p.y(), p.z());
  
  for(int i=0; i<3; i++){
    // Not ready to compute the fourth calibration point
    if(calibrationPoints[i].sqlength()<sq(0.05))
      return;
  }
  calibrationPoints[3] = calibrationPoints[2] + calibrationPoints[0] - calibrationPoints[1];
  origin = calibrationPoints[1];
  displayNormal = ((calibrationPoints[2]-calibrationPoints[1]).cross(calibrationPoints[0]-calibrationPoints[1])).norm();
  v1 = calibrationPoints[2]-calibrationPoints[1];
  v1 = v1/v1.sqlength();
  v2 = calibrationPoints[0]-calibrationPoints[1];
  v2 = v2/v2.sqlength();
}

void PointingEngine::updatePointingTarget()
{
  if(displayMode==0){
    // 1D display
    //TODO
  }else{
    // 2D display
    // Intersection of line with plane. see: http://en.wikipedia.org/w/index.php?title=Line-plane_intersection&oldid=420198019#Algebraic_form
    double d = (origin-shoulderLoc).dot(displayNormal)/pointingVector.dot(displayNormal);
    target3d = d*pointingVector+shoulderLoc;
    target = vector2d(v1.dot(target3d-origin), v2.dot(target3d-origin));
  }
}


void PointingEngine::saveCalibration()
{
  FILE* fid = fopen("calibration.txt", "w");
  if(fid==NULL)
    return;
  bool error = false;
  for(int i=0; i<3; i++){
    fprintf(fid, "Point%d:%f,%f,%f\n",i,V3COMP(calibrationPoints[i]));
  }
  fclose(fid);
  return;
}

bool PointingEngine::loadCalibration()
{
  static const bool debug = true;
  FILE* fid = fopen("calibration.txt", "r");
  if(fid==NULL)
    return false;
  bool error = false;
  for(int i=0; i<3; i++){
    double x=0,y=0,z=0;
    char buf[1024];
    sprintf(buf, "Point%d:%%lf,%%lf,%%lf\n", i);
    error = (fscanf(fid, buf,&x,&y,&z) != 3);
    calibrationPoints[i].set(x,y,z);
    if(debug) printf("read calibration point %d:%f,%f,%f, error:%d\n",i,V3COMP(calibrationPoints[i]),error?1:0);
  }
  fclose(fid);
  if(!error){
    calibrationPoints[3] = calibrationPoints[2] + calibrationPoints[0] - calibrationPoints[1];
    origin = calibrationPoints[1];
    displayNormal = ((calibrationPoints[2]-calibrationPoints[1]).cross(calibrationPoints[0]-calibrationPoints[1])).norm();
    v1 = calibrationPoints[2]-calibrationPoints[1];
    v1 = v1/v1.sqlength();
    v2 = calibrationPoints[0]-calibrationPoints[1];
    v2 = v2/v2.sqlength();
  }
  return !error;
}